
#include <WinSock2.h>
#include "protocol.h"

Protocol::Protocol()
{
	step = STEP_HEAD1;
	cnt = 0;
	ptl = { 0 };
}

Protocol::~Protocol()
{
}

uint16_t protocol_cal_crc(uint8_t* ptr, uint16_t len)
{
	unsigned short crc = 0;
	while (len--) {
		int i;
		crc ^= *ptr++ << 8;

		for (i = 0; i < 8; ++i) {
			if (crc & 0x8000)
				crc = (crc << 1) ^ 0x1021;
			else
				crc = crc << 1;
		}
	}
	return crc;
}

protocol_data_t *Protocol::parse(uint8_t data, protocol_t* p)
{
	switch (step)
	{
	case STEP_HEAD1:
		if (data == PROTOCOL_HEAD1)
		{
			cnt = 0;
			ptl.buff[cnt++] = data;
			step = STEP_HEAD2;
		}
		break;
	case STEP_HEAD2:
		if (data == PROTOCOL_HEAD2)
		{
			ptl.buff[cnt++] = data;
			step = STEP_CMD;
		}
		else
		{
			step = STEP_HEAD1;
		}
		break;
	case STEP_CMD:
		step = STEP_LEN;
		ptl.buff[cnt++] = data;
		break;
	case STEP_LEN:
		ptl.buff[cnt++] = data;
		if (cnt >= offsetof(protocol_t, data))
		{
			if (ptl.pack.len < PROTOCOL_BUFFER_LEN)
			{
				step = STEP_DATA;
			}
			else
			{
				step = STEP_HEAD1;
			}
		}
		break;
	case STEP_DATA:
		ptl.buff[cnt++] = data;

		if (cnt >= ptl.pack.len)
		{
			step = STEP_HEAD1;
			uint16_t crc = protocol_cal_crc(ptl.buff, cnt - PROTOCOL_CRC_LEN);
			if (crc == ((ptl.buff[cnt - 1] << 8) + ptl.buff[cnt - 2]))
			{
				return &ptl;
			}
		}
		break;

	default:
		step = STEP_HEAD1;
		break;
	}
	return NULL;
}

uint16_t Protocol::load(uint8_t* buf, uint8_t cmd, uint8_t* data, uint16_t len)
{
	uint16_t count = 0;
	uint16_t crc = 0;

	buf[count++] = PROTOCOL_HEAD1;
	buf[count++] = PROTOCOL_HEAD2;
	buf[count++] = cmd;
	buf[count++] = len & 0xFF;
	buf[count++] = (len >> 8) & 0xFF;
	memcpy(&buf[count], data, len);
	count += len;
	crc = protocol_cal_crc(buf, len + offsetof(protocol_t, data));
	buf[count++] = crc & 0xFF;
	buf[count++] = (crc >> 8) & 0xFF;

	return count;
}
